#ifndef __edge_following_h__
#define __edge_following_h__

#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <chrono>
#include <random>
#include "interface.h"

// 顶层状态：空闲/执行/错误
enum class EdgeMode
{
    IDLE,       // 空闲（初始状态）
    EXECUTING,  // 正常执行（步骤流转）
    ERROR       // 错误状态（触发错误处理）
};

// 沿边核心步骤类型（含错误处理）
enum class EdgeStepType
{
    FIND_BOUND,     // 寻找边界
    FOLLOW_BOUND,   // 沿边行走
    TURN_LEFT,      // 左转（工程必需，无失败状态，仅动作执行）
    TURN_RIGHT,     // 右转（工程必需，无失败状态，仅动作执行）
    OBSTACLE_AVOID, // 避障
    ERROR_PROCESS,  // 错误处理（新增核心步骤）
    END_BOUND       // 结束边界
};

// -------------------------- 1. 基础步骤：错误处理（核心新增） --------------------------
class ErrorProcessStep : public Step
{
public:
    ErrorProcessStep(IWorkflowNotify *notify, const std::string &error_reason) 
        : Step("ErrorProcess", notify, 0), error_reason_(error_reason) 
    {
        initHandlers();
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        switch (status)
        {
        case 0: return "错误识别";
        case 1: return "紧急停止";
        case 2: return "状态重置";
        case 3: return "错误处理完成";
        default: return "未知";
        }
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == 3; // 状态3：错误处理完成
    }

    unsigned int getFailedStatus() const override { return 3; } // 错误处理无“失败”，仅“完成”

private:
    void initHandlers()
    {
        // 状态0：错误识别（打印错误原因）
        status_handlers_[0] = [this](unsigned &next, int &prog)
        {
            RCLCPP_ERROR(rclcpp::get_logger(name_.c_str()), "[%s] 错误原因：%s", 
                         name_.c_str(), error_reason_.c_str());
            prog = 25;
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            next = 1;
            return true;
        };

        // 状态1：紧急停止（工程动作：切断动力）
        status_handlers_[1] = [this](unsigned &next, int &prog)
        {
            RCLCPP_WARN(rclcpp::get_logger(name_.c_str()), "[%s] 执行紧急停止...", name_.c_str());
            prog = 50;
            std::this_thread::sleep_for(std::chrono::milliseconds(800));
            next = 2;
            return true;
        };

        // 状态2：状态重置（恢复至初始空闲状态）
        status_handlers_[2] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 重置沿边状态至空闲...", name_.c_str());
            prog = 100;
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
            next = 3;
            return true;
        };
    }

private:
    std::string error_reason_; // 错误原因（来自前序失败步骤）
};

// -------------------------- 2. 核心步骤：寻找边界（无冗余失败状态） --------------------------
enum FindBoundStatus
{
    FIND_SLOW,     // 减速
    FIND_APPROACH, // 位置逼近
    FIND_CONFIRM   // 位置确认
};

class FindBoundStep : public Step
{
public:
    FindBoundStep(IWorkflowNotify *notify) : Step("FindBound", notify, FIND_SLOW) 
    {
        initHandlers();
        // 随机数初始化（模拟10%概率触发错误，符合实际故障场景）
        rng_ = std::mt19937(rd_());
        error_dist_ = std::uniform_int_distribution<>(1, 10);
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        switch ((FindBoundStatus)status)
        {
        case FIND_SLOW: return "减速";
        case FIND_APPROACH: return "位置逼近";
        case FIND_CONFIRM: return "位置确认";
        default: return "未知";
        }
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == FIND_CONFIRM;
    }

    unsigned int getFailedStatus() const override { return 999; } // 自定义错误标识

private:
    void initHandlers()
    {
        status_handlers_[FIND_SLOW] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 减速准备逼近边界...", name_.c_str());
            prog = 20;
            std::this_thread::sleep_for(std::chrono::seconds(1));
            next = FIND_APPROACH;
            // 模拟随机错误（10%概率）
            if (error_dist_(rng_) == 1) return false; 
            return true;
        };

        status_handlers_[FIND_APPROACH] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 执行位置逼近（修正坐标）...", name_.c_str());
            prog = 60;
            std::this_thread::sleep_for(std::chrono::seconds(2));
            next = FIND_CONFIRM;
            return true;
        };

        status_handlers_[FIND_CONFIRM] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 验证边界位置精度（误差<5cm）...", name_.c_str());
            prog = 100;
            return true;
        };
    }

private:
    std::random_device rd_;
    std::mt19937 rng_;
    std::uniform_int_distribution<> error_dist_; // 模拟错误概率
};

// -------------------------- 3. 核心步骤：沿边行走 --------------------------
enum FollowBoundStatus
{
    FOLLOW_SLOW,   // 减速
    FOLLOW_ALIGN,  // 姿态对齐
    FOLLOW_TRACK   // 沿边跟踪
};

class FollowBoundStep : public Step
{
public:
    FollowBoundStep(IWorkflowNotify *notify) : Step("FollowBound", notify, FOLLOW_SLOW) 
    {
        initHandlers();
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        switch ((FollowBoundStatus)status)
        {
        case FOLLOW_SLOW: return "减速";
        case FOLLOW_ALIGN: return "姿态对齐";
        case FOLLOW_TRACK: return "沿边跟踪";
        default: return "未知";
        }
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == FOLLOW_TRACK;
    }

    unsigned int getFailedStatus() const override { return 999; }

private:
    void initHandlers()
    {
        status_handlers_[FOLLOW_SLOW] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 减速至沿边速度（0.3m/s）...", name_.c_str());
            prog = 20;
            std::this_thread::sleep_for(std::chrono::milliseconds(800));
            next = FOLLOW_ALIGN;
            return true;
        };

        status_handlers_[FOLLOW_ALIGN] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 对齐边界（保持距离10cm）...", name_.c_str());
            prog = 50;
            std::this_thread::sleep_for(std::chrono::milliseconds(1200));
            next = FOLLOW_TRACK;
            return true;
        };

        status_handlers_[FOLLOW_TRACK] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 沿边跟踪（闭环控制修正偏差）...", name_.c_str());
            prog = 100;
            std::this_thread::sleep_for(std::chrono::seconds(3));
            return true;
        };
    }
};

// -------------------------- 4. 基础步骤：避障（触发错误概率） --------------------------
class ObstacleAvoidStep : public Step
{
public:
    ObstacleAvoidStep(IWorkflowNotify *notify) : Step("ObstacleAvoid", notify, 0) 
    {
        initHandlers();
        rng_ = std::mt19937(rd_());
        error_dist_ = std::uniform_int_distribution<>(1, 15); // 7%错误概率
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        switch (status)
        {
        case 0: return "障碍物检测";
        case 1: return "绕行路径规划";
        case 2: return "执行绕行";
        default: return "未知";
        }
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == 2;
    }

    unsigned int getFailedStatus() const override { return 999; }

private:
    void initHandlers()
    {
        status_handlers_[0] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 检测到障碍物（距离1.2m）...", name_.c_str());
            prog = 30;
            std::this_thread::sleep_for(std::chrono::milliseconds(600));
            next = 1;
            // 模拟避障错误
            if (error_dist_(rng_) == 1) return false;
            return true;
        };

        status_handlers_[1] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 规划绕行路径（左转0.5m）...", name_.c_str());
            prog = 60;
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
            next = 2;
            return true;
        };

        status_handlers_[2] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 执行绕行（完成后回归沿边）...", name_.c_str());
            prog = 100;
            std::this_thread::sleep_for(std::chrono::seconds(2));
            return true;
        };
    }

private:
    std::random_device rd_;
    std::mt19937 rng_;
    std::uniform_int_distribution<> error_dist_;
};

// -------------------------- 5. 基础步骤：左转/右转（仅动作，无失败） --------------------------
class TurnLeftStep : public Step
{
public:
    TurnLeftStep(IWorkflowNotify *notify) : Step("TurnLeft", notify, 0) 
    {
        initHandlers();
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        return status == 0 ? "执行左转（90°）" : "左转完成";
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == 1;
    }

    unsigned int getFailedStatus() const override { return 999; }

private:
    void initHandlers()
    {
        status_handlers_[0] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 执行左转（速度0.2m/s，角度90°）...", name_.c_str());
            prog = 100;
            std::this_thread::sleep_for(std::chrono::seconds(1));
            next = 1;
            return true;
        };
    }
};

class TurnRightStep : public Step
{
public:
    TurnRightStep(IWorkflowNotify *notify) : Step("TurnRight", notify, 0) 
    {
        initHandlers();
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        return status == 0 ? "执行右转（90°）" : "右转完成";
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == 1;
    }

    unsigned int getFailedStatus() const override { return 999; }

private:
    void initHandlers()
    {
        status_handlers_[0] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 执行右转（速度0.2m/s，角度90°）...", name_.c_str());
            prog = 100;
            std::this_thread::sleep_for(std::chrono::seconds(1));
            next = 1;
            return true;
        };
    }
};

// -------------------------- 6. 基础步骤：结束边界 --------------------------
class EndBoundStep : public Step
{
public:
    EndBoundStep(IWorkflowNotify *notify) : Step("EndBound", notify, 0) 
    {
        initHandlers();
    }

protected:
    std::string statusToString(unsigned int status) const override
    {
        switch (status)
        {
        case 0: return "减速停止";
        case 1: return "结束确认";
        default: return "未知";
        }
    }

    bool isCompleteStatus(unsigned int status) const override
    {
        return status == 1;
    }

    unsigned int getFailedStatus() const override { return 999; }

private:
    void initHandlers()
    {
        status_handlers_[0] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 减速至停止（0m/s）...", name_.c_str());
            prog = 50;
            std::this_thread::sleep_for(std::chrono::seconds(1));
            next = 1;
            return true;
        };

        status_handlers_[1] = [this](unsigned &next, int &prog)
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 沿边任务结束，进入空闲状态...", name_.c_str());
            prog = 100;
            return true;
        };
    }
};

// -------------------------- 顶层工作流：步骤调度+错误跳转 --------------------------
class EdgeFollowingWorkflow : public IWorkflowNotify
{
public:
    EdgeFollowingWorkflow(const std::string &name) : name_(name)
    {
        // 1. 初始化ROS2节点与定时器（100ms调度）
        node_ = rclcpp::Node::make_shared("edge_following_node");
        timer_ = node_->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&EdgeFollowingWorkflow::timerCallback, this)
        );

        // 2. 初始化正常步骤序列（按沿边流程排序）
        initNormalSteps();
    }

    // 启动沿边任务
    void start()
    {
        mode_ = EdgeMode::EXECUTING;
        current_step_idx_ = 0;
        RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 沿边任务启动，进入执行状态", name_.c_str());
    }

    // 获取ROS2节点（供main函数spin）
    std::shared_ptr<rclcpp::Node> get_node() const { return node_; }

private:
    // 初始化正常步骤序列（实际沿边流程）
    void initNormalSteps()
    {
        normal_steps_.clear();
        normal_steps_.push_back(std::make_shared<FindBoundStep>(this));     // 1. 寻找边界
        normal_steps_.push_back(std::make_shared<FollowBoundStep>(this));   // 2. 沿边行走
        normal_steps_.push_back(std::make_shared<ObstacleAvoidStep>(this)); // 3. 避障（模拟错误）
        normal_steps_.push_back(std::make_shared<TurnLeftStep>(this));      // 4. 左转
        normal_steps_.push_back(std::make_shared<FollowBoundStep>(this));   // 5. 继续沿边
        normal_steps_.push_back(std::make_shared<EndBoundStep>(this));      // 6. 结束任务
    }

    // 定时器回调：核心调度逻辑（含错误跳转）
    void timerCallback()
    {
        // 1. 空闲状态：不执行
        if (mode_ == EdgeMode::IDLE) return;

        // 2. 错误状态：执行错误处理步骤
        if (mode_ == EdgeMode::ERROR)
        {
            executeErrorStep();
            return;
        }

        // 3. 正常执行状态：调度正常步骤
        if (current_step_idx_ >= normal_steps_.size())
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 所有正常步骤执行完成，回归空闲", name_.c_str());
            mode_ = EdgeMode::IDLE;
            return;
        }

        // 执行当前正常步骤
        auto &current_step = normal_steps_[current_step_idx_];
        int progress = 0;
        bool success = current_step->execute(progress);

        // 步骤失败：切换至错误状态，创建错误处理步骤
        if (!success)
        {
            std::string error_reason = "步骤[" + current_step->getName() + "]执行失败";
            error_step_ = std::make_shared<ErrorProcessStep>(this, error_reason);
            mode_ = EdgeMode::ERROR;
            RCLCPP_ERROR(rclcpp::get_logger(name_.c_str()), "[%s] %s，触发错误处理", name_.c_str(), error_reason.c_str());
            return;
        }

        // 步骤成功：切换至下一个正常步骤
        if (current_step->isCompleted())
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 步骤完成：%s（进度：%d%%）",
                        name_.c_str(), current_step->getName().c_str(), progress);
            current_step_idx_++;
        }
    }

    // 执行错误处理步骤
    void executeErrorStep()
    {
        if (!error_step_) return;

        int progress = 0;
        bool success = error_step_->execute(progress);

        // 错误处理完成：回归正常步骤初始化，进入空闲状态
        if (error_step_->isCompleted())
        {
            RCLCPP_INFO(rclcpp::get_logger(name_.c_str()), "[%s] 错误处理完成，可重新启动任务", name_.c_str());
            initNormalSteps(); // 重置正常步骤
            current_step_idx_ = 0;
            mode_ = EdgeMode::IDLE;
            error_step_.reset(); // 释放错误步骤
        }
    }

    // 实现IWorkflowNotify接口：步骤失败回调（冗余保障）
    void onStepFailed(const std::string &step_name, const std::string &reason) override
    {
        std::string error_reason = "步骤[" + step_name + "]回调失败：" + reason;
        error_step_ = std::make_shared<ErrorProcessStep>(this, error_reason);
        mode_ = EdgeMode::ERROR;
        RCLCPP_ERROR(rclcpp::get_logger(name_.c_str()), "[%s] %s，触发错误处理", name_.c_str(), error_reason.c_str());
    }

    // 实现IWorkflowNotify接口：获取工作流名称
    std::string getWorkflowName() const override { return name_; }

private:
    std::shared_ptr<rclcpp::Node> node_;                // ROS2节点
    rclcpp::TimerBase::SharedPtr timer_;                // 100ms定时器
    EdgeMode mode_ = EdgeMode::IDLE;                    // 顶层状态
    std::vector<std::shared_ptr<Step>> normal_steps_;   // 正常步骤序列
    std::shared_ptr<Step> error_step_;                  // 错误处理步骤（动态创建）
    size_t current_step_idx_ = 0;                       // 正常步骤索引
    std::string name_;                                  // 工作流名称
};

#endif